
#include <Arduino.h>
#include <TimerOne.h>
#include <ArduinoJson.h>
#include <Arduino_JSON.h>
#define PACKET_HEADER 0x0C
#define PACKET_FOOTER 0xA5
#define MAX_PACKET_LENGTH 32
#define JSON_DOC_SIZE 256

uint8_t packetBuffer[MAX_PACKET_LENGTH];
uint8_t packetIndex = 0;
bool packetStarted = false;
String jsonString;

String inputString = ""; 
String before= "";

int lstopflage=1;
int zstopflage=1;
int rstopflage=1;

//三面舵初始化数据
uint8_t zero[14] =       {0x24,0x4C,0x44,0x58,0x41,0x2C,0x20,0x30,0x30,0x2E,0x30,0x2A,0x30,0x33};     //rpm
uint8_t tempjiaodu[14] = {0x24,0x4C,0x44,0x58,0x41,0x2C,0x2B,0x30,0x30,0x2E,0x30,0x2A,0x30,0x33};

 int absValue =0;
  // 准备存储ASCII结果的数组


//报警端子为

//驾机联动端子 D32
const int switchPin = 32;  // 驾机联动端子 D32

const int z1 = 7;  // 报警

const int z2 = 34;  // 紧急停止
const int z3 = 36;  // 停止2
const int z4 = 38;  // 启动4

const int m1 = 9;  // 报警

const int m2 = 40;  // 紧急停止
const int m3 = 42;  // 停止6
const int m4 = 44;  // 启动8

const int r1 = 11;  // 报警

const int r2 = 45;  // 紧急停止
const int r3 = 46;  // 停止10
const int r4 = 47;  // 启动12


JSONVar root;
JSONVar arr = JSONVar::parse("[\"1\",\"1\",\"0\",\"0\",\"0\",\"0\"]");
JSONVar DW = JSONVar::parse("[\"N\",\"N\",\"N\"]");




const int FILTER_N = 6; // 每次滤波采样次数
const int NUM_CHANNELS = 6;

const int ANALOG_PINS[NUM_CHANNELS] = {2, 4, 6, 8, 10, 12};  // 模拟输入引脚数组
const unsigned long TRIGGER_DURATION = 1200; // 触发持续时间1秒

// 全局变量
volatile int voltageStates[NUM_CHANNELS] = {1, 1, 1, 1, 1, 1};  // 电压状态数组，初始为1
volatile unsigned long lowVoltageStartTimes[NUM_CHANNELS] = {0, 0, 0, 0, 0, 0};  // 各通道低电压开始时间
volatile bool isLowVoltage[NUM_CHANNELS] = {false, false, false, false, false, false};  // 低电压状态标志

void setup() {

pinMode(switchPin, INPUT_PULLUP);//驾机联动
  
pinMode(z2, INPUT_PULLUP);
pinMode(z3, OUTPUT);
pinMode(z4, OUTPUT);

pinMode(m2, INPUT_PULLUP);
pinMode(m3, OUTPUT);
pinMode(m4, OUTPUT);

pinMode(r2, INPUT_PULLUP);
pinMode(r3, OUTPUT);
pinMode(r4, OUTPUT);

  Serial.begin(115200);
  Serial1.begin(115200);
  Serial2.begin(115200);
   // 初始化JSON
  root["FrontT"] = 0;
  root["BackT"] = 0;
  root["RCHE"] = arr;
  root["ZCHE"] = arr;
  root["LCHE"] = arr;
  root["D"] = 0;
  root["JJLD"] = "FALSE";
  root["ZHBJ"] = "123";
  root["DW"] = DW;



//三面舵机初始化0
Serial3.begin(4800);
pinMode(13, OUTPUT);
digitalWrite(13, HIGH);
Serial3.write(zero,14);
Serial3.flush(); // 必须等到发送完成
digitalWrite(13, LOW);
delay(100);

digitalWrite(z3, HIGH);
digitalWrite(r3, HIGH);
digitalWrite(m3, HIGH);


// 初始化定时器中断，每100ms执行一次checkVoltage
  Timer1.initialize(30 * 1000); // 转换为微秒
  Timer1.attachInterrupt(checkAllVoltages);




}

void loop() {

 
while (Serial2.available()) {
    uint8_t receivedByte = Serial2.read();
    
    if (receivedByte == PACKET_HEADER && !packetStarted) {
      packetStarted = true;
      packetIndex = 0;
      packetBuffer[packetIndex++] = receivedByte;
    } 
    else if (packetStarted) {
      packetBuffer[packetIndex++] = receivedByte;
      
      if (receivedByte == PACKET_FOOTER) {
        printHexPacket();
        packetStarted = false;
      }
      else if (packetIndex >= MAX_PACKET_LENGTH) {
        packetStarted = false;
      }
    }
    receivedByte=NULL;
  }

  
//READ 舵机 getMedianAverage
root["D"] = 512-getMedianAverage(A0);

//read 按键

   readkey();

   ReceSerial1();
     if((inputString != "")&&(before != inputString)){
      before=inputString;
         Controlmechine(before.toInt());
      }

jsonString= JSON.stringify(root);
Serial.println(jsonString);
Serial1.println(jsonString);
    delay(150);
   inputString="";


    //  test  主循环定期显示当前状态
 /*static unsigned long lastDisplayTime = 0;
  if (millis() - lastDisplayTime >= 1000) {
    displayCurrentStates();
    lastDisplayTime = millis();
  }
  
  delay(10); // 保持系统响应性
  */
   
}



int getMedianAverage(int pin) {
  int i, j;
  int value_buf[FILTER_N];
  int temp;
  
  // 采样数据
  for (i = 0; i < FILTER_N; i++) {
    value_buf[i] = analogRead(pin);
    delay(1);
  }
  
  // 冒泡排序
  for (j = 0; j < FILTER_N - 1; j++) {
    for (i = 0; i < FILTER_N - j - 1; i++) {
      if (value_buf[i] > value_buf[i + 1]) {
        temp = value_buf[i];
        value_buf[i] = value_buf[i + 1];
        value_buf[i + 1] = temp;
      }
    }
  }
  
  // 去掉最大最小值后求平均（中位值平均）
  long sum = 0;
  for (i = 1; i < FILTER_N - 1; i++) {
    sum += value_buf[i];
  }
  return sum / (FILTER_N - 2);
}


void ReceSerial1(){

  while (Serial1.available())  
    {   
        char inChar = (char)Serial1.read();
        inputString += inChar;
        if (inChar == '\n')break;
    }

    
}


// 定时器中断服务程序 - 监测所有通道电压
void checkAllVoltages() {
  
  for (int i = 0; i < NUM_CHANNELS; i++) {
    // 读取模拟输入值并转换为电压
   
    float voltage = analogRead(ANALOG_PINS[i]);
    
    // 检查电压是否低于阈值
    if (voltage < 12) {
      if (!isLowVoltage[i]) {
        // 首次检测到低电压，记录开始时间
        lowVoltageStartTimes[i] = millis();
        isLowVoltage[i] = true;
      } else {
        // 持续低电压状态，检查是否达到2秒
        unsigned long currentDuration = millis() - lowVoltageStartTimes[i];
        if (currentDuration >= TRIGGER_DURATION) {
          voltageStates[i] = 0;  // 持续2秒低于1V，状态置为0
          lowVoltageStartTimes[i] = millis(); // 重置计时器
        }
      }
    } else {
      // 电压恢复正常，重置状态并置为1
      if (isLowVoltage[i]) {
        isLowVoltage[i] = false;
      }
      voltageStates[i] = 1;  // 电压正常，状态置为1
    }
  }


}



void readkey(){


  if (digitalRead(switchPin) == LOW) { //驾机联动 驾机联动端子 D32
     root["JJLD"] = "TRUE";
  }else
  {
     root["JJLD"] = "FALSE";
  }
//左


  
//处理灯的逻辑
if (digitalRead(z2) == LOW) { //紧急停止
     root["LCHE"][3] = "2";
     root["LCHE"][5]="0";
  }else
{
  root["LCHE"][3] = "0";
}

//处理一下同时按下

if ((voltageStates[0] ==0 ) &&(voltageStates[1] ==1)){ //停止
  root["LCHE"][2] = "0";
  digitalWrite(z3, HIGH);
  digitalWrite(z4, LOW);
  root["LCHE"][5]="0";
   lstopflage=1;
  }

  if((voltageStates[1] ==0 ) &&(voltageStates[0] ==1)){ //启动
  root["LCHE"][2] = "1";
  digitalWrite(z4, HIGH);
  digitalWrite(z3, LOW);
  lstopflage=0;

  }
//中
//处理灯的逻辑
if (digitalRead(m2) == LOW) { //紧急停止
     root["ZCHE"][3] = "2";
     root["ZCHE"][5]="0";
  }else
  {
    root["ZCHE"][3] = "0";
  }


//处理一下同时按下
if ((voltageStates[2] ==0 ) &&(voltageStates[3] ==1)) { //停止
     root["ZCHE"][2] = "0";
     digitalWrite(m3, HIGH);
     digitalWrite(m4, LOW);
     root["ZCHE"][5]="0";
      zstopflage=1;
  }

  if ((voltageStates[3] ==0 ) &&(voltageStates[2] ==1)){ //启动
     root["ZCHE"][2] = "1";
    digitalWrite(m4, HIGH);
    digitalWrite(m3, LOW);
    zstopflage=0;
  }
//右



//处理灯的逻辑

if (digitalRead(r2) == LOW) { //紧急停止
     root["RCHE"][3] = "2";
     root["RCHE"][5]="0";
  }else
  {
    root["RCHE"][3] = "0";
  }

if ((voltageStates[4] ==0 ) &&(voltageStates[5] ==1)) { //停止
     root["RCHE"][2] = "0";
      digitalWrite(r3, HIGH);
     digitalWrite(r4, LOW);
     root["RCHE"][5]="0";
      rstopflage=1;
  }

  if ((voltageStates[5] ==0 ) &&(voltageStates[4] ==1)) { //启动
     root["RCHE"][2] = "1";
    digitalWrite(r4, HIGH);
    digitalWrite(r3, LOW);
    rstopflage=0;
  }




if (analogRead(z1) <= 50) { //报警按下
    root["ZHBJ"] = "100";
    return;
  }else
  {
     root["ZHBJ"] = "123";
    
  }

if (analogRead(m1) <= 50) { //报警按下
    root["ZHBJ"] = "010";
    return;
  }else
  {
     root["ZHBJ"] = "123";
  }


if (analogRead(r1) <= 50) { //报警按下
    root["ZHBJ"] = "001";
    return;
  }else
  {
     root["ZHBJ"] = "123";
  }


}
 

void Controlmechine(int ang){


if(ang>=35){
  ang=35;
  }

  
if(ang<=-35){
    ang=-35;   
  }


if(ang!=0){ //right green
  ASSICTo16_ROT(ang);
  digitalWrite(13, HIGH);

  Serial3.write(tempjiaodu,14);
  Serial3.flush(); // 必须等到发送完成
  digitalWrite(13, LOW);
   return;
  }



  //等于0
if(ang==0){
  digitalWrite(13, HIGH);
  Serial3.write(zero,14);
  Serial3.flush(); // 必须等到发送完成
  digitalWrite(13, LOW);
  return;
  }
}

void ASSICTo16_ROT(int ang)
{
if(ang>0){
tempjiaodu[6]=0x2B;
}
if(ang<0){ //right green
tempjiaodu[6]=0x2D;
}
 

  absValue = abs(ang);
  // 准备存储ASCII结果的数组
char result[3] = {'0', '0', '\0'}; // 初始化两位数字符串
  
  if(absValue < 10) {
    // 小于10时补零
    result[1] = absValue + '0'; // 转ASCII
  } else {
    // 大于等于10时直接转换
    result[0] = (absValue / 10) + '0';
    result[1] = (absValue % 10) + '0';
  }

  // 转换为16进制存储到第7、8位(索引6、7)
  tempjiaodu[7] = (uint8_t)result[0]; // 存储ASCII字符的高位
  tempjiaodu[8] = (uint8_t)result[1]; // 存储ASCII字符的低位

  // 计算BCC校验（跳过第0位和第11位）
  uint8_t bcc = 0;
  for(int i = 1; i < 11; i++) {
      bcc ^= tempjiaodu[i];
  }
   // 获取低4位并转换为ASCII字符
  uint8_t lowNibble = bcc & 0x0F;
  char asciiChar = (lowNibble < 10) ? (lowNibble + '0') : (lowNibble - 10 + 'A');
  
  // 将ASCII字符转换回HEX存入12、13位
  tempjiaodu[12] = 0x30;
  tempjiaodu[13] = asciiChar;
  

}






void printHexPacket() {
 
 
  // 检查数据包长度有效性
    if (packetIndex < 8){
    return;
  }
  // 判断第4字节(索引3)//左车
  if (packetBuffer[3] == 0x01) {
      chedataRead("Left");
  } 
  else if (packetBuffer[3] == 0x02) //中车 右车
  {
      chedataRead("Right");
  }

  
}

void chedataRead(String leftRight)
{
// 判断左中右
//中右
if((leftRight=="Left")&&((packetBuffer[10]==3)||(packetBuffer[10]==1))){
 
  if(lstopflage==0)
  {
      // 判断第6字节(索引5)// 左
      switch (packetBuffer[5])
      {
        case 0x00:  root["LCHE"][5] =  "0"; root["DW"][0] = "N";break;
        case 0x02:  root["LCHE"][5]  = "0"; root["DW"][0] = "D";break;
        case 0x04:  root["LCHE"][5]  = "0"; root["DW"][0] = "R";break;
        //问李明月  是否对车进行平滑处理
        case 0x01:  root["LCHE"][5]  = String(int(packetBuffer[6])+25); root["DW"][0] = "D";break;
        case 0x08:  root["LCHE"][5] = String(-int(packetBuffer[6])-25); root["DW"][0] = "R";break;
        default: Serial.print(packetBuffer[1]);
      }
  }

}

//中右
if(leftRight=="Right"){
    // 判断第6字节(索引5)// 中
    if(zstopflage==0)
    {
        if((packetBuffer[10]==3)||(packetBuffer[10]==1))
        {
          switch (packetBuffer[5]) 
          {
            case 0x00:  root["ZCHE"][5] = "0"; root["DW"][1] = "N";break;
            case 0x02:  root["ZCHE"][5] = "0"; root["DW"][1] = "D";break;
            case 0x04:  root["ZCHE"][5] = "0"; root["DW"][1] = "R";break;
             //问李明月  是否对车进行平滑处理
            case 0x01:  root["ZCHE"][5] = String(int(packetBuffer[6])+25); root["DW"][1] = "D";break;
            case 0x08:  root["ZCHE"][5] = String(-int(packetBuffer[6])-25); root["DW"][1] = "R";break;
            default: Serial.print("Byte6: Unknown value");
          }
        }
    }
    // 右
    if(rstopflage==0)
    {
        if((packetBuffer[10]==3)||(packetBuffer[10]==2))
        {
          switch (packetBuffer[7])
          {
            case 0x00:  root["RCHE"][5] = "0"; root["DW"][2] = "N";break;
            case 0x02:  root["RCHE"][5]  = "0"; root["DW"][2] = "D";break;
            case 0x04:  root["RCHE"][5]  = "0"; root["DW"][2]= "R";break;
             //问李明月  是否对车进行平滑处理
            case 0x01:  root["RCHE"][5]  = String(int(packetBuffer[8])+25);  root["DW"][2] = "D";break;
            case 0x08:  root["RCHE"][5] = String(-int(packetBuffer[8])-25); root["DW"][2]= "R";break;
            default: Serial.print("Byte6: Unknown value");
          }
        }
    }
  }
}


// 显示当前所有通道状态
void displayCurrentStates() {
  Serial.print("时间: ");
  Serial.print(millis() / 1000);
  Serial.print("s | 状态: [");
  
  for (int i = 0; i < NUM_CHANNELS; i++) {
    Serial.print(voltageStates[i]);
    if (i < NUM_CHANNELS - 1) Serial.print(", ");
  }
  Serial.println("]");
}
